/* *********************************************************************** *
 * project: org.matsim.*
 * EnvironmentForceModule.java
 *                                                                         *
 * *********************************************************************** *
 *                                                                         *
 * copyright       : (C) 2007 by the members listed in the COPYING,        *
 *                   LICENSE and WARRANTY file.                            *
 * email           : info at matsim dot org                                *
 *                                                                         *
 * *********************************************************************** *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *   See also COPYING, LICENSE and WARRANTY file                           *
 *                                                                         *
 * *********************************************************************** */
package aiingames.simulation.server.physics;

import aiingames.Config;

import com.vividsolutions.jts.geom.Coordinate;


/**
 * Environment interaction forces according to: D. Helbing, I. Farkas, T. Vicsek,
 * Nature 407, 487-490 (2000)
 * 
 * @author laemmel
 * 
 */
public class EnvironmentForceModule implements ForceModule {



	//Helbing constants 
	private static final double Bi=0.08;
	private static final double Ai=1130;
	private static final double k = 1.2 * 100000;
	private static final double kappa = 2.4 * 100000;
	private StaticEnvironmentDistancesField envDists;

	/**
	 * @param floor
	 * @param scenario
	 */
	public EnvironmentForceModule(StaticEnvironmentDistancesField envDists) {
		this.envDists = envDists;
	}

	@Override
	public void run(Particle particle) {
		double fx = 0;
		double fy = 0;

		
		
		EnvironmentDistances ed = this.envDists.getEnvironmentDistances(particle.getPosition());
		for (Coordinate obj : ed.getObjects()) {
			double dist = obj.distance(particle.getPosition());
			if (dist > Config.PNeighborhoddRange) {
				continue;
			}
			double dx =(particle.getPosition().x - obj.x) / dist;
			double dy =(particle.getPosition().y - obj.y) / dist;

			double bounderyDist = particle.getDiameter()/4 - dist;
			double g = bounderyDist > 0 ? bounderyDist : 0;

			double tanDvx = (- particle.getVx()) * dx;
			double tanDvy = (- particle.getVy()) * dy;

			double tanX = tanDvx * -dx;
			double tanY = tanDvy * dy;

			double xc = (Ai * Math.exp((bounderyDist) / Bi) + k*g)* dx+ kappa * g * tanX;
			double yc = (Ai * Math.exp((bounderyDist) / Bi) + k*g)* dy + kappa * g * tanY;

			fx += xc;
			fy += yc;

		}


		particle.getForce().incrementX(fx);
		particle.getForce().incrementY(fy);

	}

}
